# 导入所需库
import RPi.GPIO as GPIO  # 树莓派GPIO控制库
import time              # 时间相关操作
import threading         # 多线程
import numpy             # 数值计算
import matplotlib.pyplot as plt  # 绘制可视化曲线

# 定义电机控制引脚（BCM编号模式）
EA, I2, I1, EB, I4, I3, LS, RS = (13, 19, 26, 16, 20, 21, 6, 12)
FREQUENCY = 50  # PWM频率50Hz

# GPIO初始化配置
GPIO.setmode(GPIO.BCM)  # 设置GPIO编号模式为BCM
GPIO.setup([EA, I2, I1, EB, I4, I3], GPIO.OUT)  # 配置电机控制引脚为输出模式
GPIO.setup([LS, RS], GPIO.IN)  # 配置传感器引脚为输入模式
# 初始化电机驱动方向
GPIO.output([EA, I2, EB, I3], GPIO.LOW)  # 设置使能端和部分方向引脚为低电平
GPIO.output([I1, I4], GPIO.HIGH)         # 向I1、I4输出高电平

# 创建PWM控制器并启动
pwma = GPIO.PWM(EA, FREQUENCY)  # 左电机PWM实例
pwmb = GPIO.PWM(EB, FREQUENCY)  # 右电机PWM实例
pwma.start(0)  # 启动PWM，初始占空比0%
pwmb.start(0)

# 全局速度变量和计数器
lspeed = 0  # 左轮速度
rspeed = 0  # 右轮速度
lcounter = 0  # 左轮触发计数
rcounter = 0  # 右轮触发计数

class PID:
    """PID Controller
    """
    def __init__(self, P=80, I=0, D=0, speed=0.4, duty=26):
        self.Kp = P  # 比例系数
        self.Ki = I  # 积分系数
        self.Kd = D  # 微分系数
        self.err_pre = 0    # 前一时刻误差 e(k-1)
        self.err_last = 0   # 前两时刻误差 e(k-2)
        self.u = 0          # 控制输出量
        self.integral = 0   # 误差积分项
        self.ideal_speed = speed  # 目标速度设定值
	
    def update(self, feedback_value):
        """更新PID状态并返回控制量"""
        self.err_last = self.err_pre  # 保存历史误差
        self.err_pre = self.ideal_speed - feedback_value  # 计算当前误差
        self.integral += self.err_pre  # 误差积分项累加
        
        # PID公式计算
        self.u = (self.Kp * self.err_pre +       # 比例项
                 self.Ki * self.integral +       # 积分项
                 self.Kd * (self.err_pre - self.err_last))  # 微分项
        
        # 输出限幅（0-100对应占空比0%-100%）
        if self.u> 100:
            self.u= 100
        elif self.u < 0:
            self.u = 0
        return self.u

    def setKp(self, proportional_gain):
        """Determines how aggressively the PID reacts to the current error with setting Proportional Gain 设置比例系数"""
        self.Kp = proportional_gain

    def setKi(self, integral_gain):
        """Determines how aggressively the PID reacts to the current error with setting Integral Gain 设置积分系数"""
        self.Ki = integral_gain

    def setKd(self, derivative_gain):
        """Determines how aggressively the PID reacts to the current error with setting Derivative Gain 设置微分系数"""
        self.Kd = derivative_gain

def my_callback(channel):
    """触发中断回调函数"""
    global lcounter, rcounter
    # 根据触发通道更新对应计数器
    if channel == LS:
        lcounter += 1  # 左轮传感器触发计数
    elif channel == RS:
        rcounter += 1  # 右轮传感器触发计数
            
def getspeed():
    """速度计算线程函数"""
    global rspeed
    global lspeed
    global lcounter
    global rcounter
    # 启用传感器边缘检测（上升沿触发）
    GPIO.add_event_detect(LS, GPIO.RISING, callback=my_callback)
    GPIO.add_event_detect(RS, GPIO.RISING, callback=my_callback)
    
    while True:
        # 计算转速：脉冲数/每转脉冲数（585为编码器每转脉冲数）
        rspeed = rcounter / 585.0  # 右轮转速（转/秒）
        lspeed = lcounter / 585.0  # 左轮转速（转/秒）
        # 重置脉冲计数器
        rcounter = 0
        lcounter = 0
        # 采样周期（0.1秒）
        time.sleep(0.1)

# 启动速度监测线程
thread1 = threading.Thread(target=getspeed)
thread1.start()

# 数据记录相关变量
i = 0  # 时间轴
x = []  # 时间序列数据
y1 = []  # 左轮速度记录
y2 = []  # 右轮速度记录
speed = 1.9  # 初始化目标速度（转/秒）
l_origin_duty = 5  # 左轮初始占空比
r_origin_duty = 5  # 右轮初始占空比

# 启动电机并初始化PID控制器
pwma.start(l_origin_duty)
pwmb.start(r_origin_duty)
L_control = PID(30, 0.06, 20, speed, l_origin_duty)  # 左轮PID参数
R_control = PID(40, 0.01, 23, speed, r_origin_duty)  # 右轮PID参数

try:
    while True:
        # 更新PWM输出值（PID控制）
        pwma.ChangeDutyCycle(L_control.update(lspeed))  # 左轮控制
        pwmb.ChangeDutyCycle(R_control.update(rspeed))  # 右轮控制
        
        # 记录数据
        x.append(i)
        y1.append(lspeed)
        y2.append(rspeed)
        
        # 控制循环周期（约0.1秒）
        time.sleep(0.1)
        i += 0.1  # 时间累加
        
        # 打印实时状态
        print('left: %f  right: %f lduty: %f rduty: %f' % 
              (lspeed, rspeed, L_control.u, R_control.u))
        
except KeyboardInterrupt:
    # 处理用户中断（Ctrl+C）
    pass

# 清理阶段
pwma.stop()  # 停止PWM输出
pwmb.stop()
# 绘制速度曲线
plt.plot(x, y1, '-o')  # 左轮速度曲线
plt.plot(x, y2, '-*')  # 右轮速度曲线
plt.show()  # 显示图表

GPIO.cleanup()  # 重置GPIO状态
plt.show()  # 显示图表